#include "WPILib.h"
#include "Defs.h"

class Robot2489 : public SimpleRobot {
	//RobotDrive myRobot; // robot drive system
	//Joystick stickLeft; // only joystick
	//Joystick stickRight;

	RobotDrive *myRobot;
	Joystick *manipulator1;
	Joystick *manipulator2;
	
	/** Will be using only one of Mecanum control or Tank control at a time**/
	//For Mecanum Drive
	Joystick *strafeStick, *spinStick;
	
	//For Game Controllers
	Joystick *game;
	
	//For Tank Drive
	Joystick *leftStick, *rightStick;
	
	DriverStationLCD *screen;
	//AxisCamera *camera;
	DriverStation *ds;
	DriverStationEnhancedIO *dseio;
	Kinect *kinect;
	bool RightStart;
	bool LeftStart;
	bool MidStart;
	bool RightBranch;
	bool Slow;
	bool EmergencyPause;
	
	// Jaguar motors
	Jaguar *leftFrontMotor;
	Jaguar *leftBackMotor;
	Jaguar *rightFrontMotor;
	Jaguar *rightBackMotor;	
	//Jaguar *topWheel;
	//Jaguar *bottomWheel;
	Jaguar *armMotor;
	
	//PID Controllers
	PIDController *leftFrontController;
	PIDController *leftBackController;
	PIDController *rightFrontController;
	PIDController *rightBackController;
	
	// Line sensors
	DigitalInput *rightLineSensor;
	DigitalInput *middleLineSensor;
	DigitalInput *leftLineSensor;
	
	//AnalogChannel
	AnalogChannel *potentiometer;
	
	//Gyro
	Gyro *gyro;
	
	// Encoders
	Encoder *leftFrontEncoder;
	Encoder *leftBackEncoder;
	Encoder *rightFrontEncoder;
	Encoder *rightBackEncoder;
	
	//Spikes
	Relay *leftSpike;
	Relay *rightSpike;
	//Relay *armSpike; 
	//Relay *topWheelSpike;
	//Relay *bottomWheelSpike;
	
	//Manipulator limit switch instantiation
	DigitalInput *bottomLimit;
	DigitalInput *topLimit;
	DigitalInput *lowOutSwitch;
	DigitalInput *lowInSwitch;
	DigitalInput *feederSwitch;
	DigitalInput *midOutSwitch;
	DigitalInput *midInSwitch;
	//Manipulator limit switch booleans
	bool onLowOutSwitch;
	bool onLowInSwitch;
	bool onMidOutSwitch;
	bool onMidInSwitch;
	bool onFeederSwitch;
	//Manipulator joystick button booleans
	bool lowOutButton;
	bool lowInButton;
	bool feederButton;
	bool midOutButton;
	bool midInButton;
	//Variables to control the manip
	int manipDirection;//determines if arm is going up or down to reach desired position
	int manipActualPeg;//Position of the arm according to the button
	int manipDesiredPeg;//Wanted position of the arm according to the buttons.
	float manipvalue;//measure value of joystick's y axis.
	float startAngle;
	
	//Timer
	Timer *timer;
	
	//Servo
	Servo *deployer;
	
public:
	
	Robot2489();
	void Autonomous();
	void OperatorControl();
	void ReadIOModule();
	void LineFollow();
	void LineV();
	void LineFollowMid();
	void Manipulator();
	void ManipMove();

	//Initializers
	void InitSpike();
	void InitJaguar();
	void InitAnalogChannel();
	void InitEncoder();
	void InitJoystick();
	void InitKinect();
	void InitLightSensor();
	//void InitCamera();
	void InitLimitSwitch();
	//void InitGyro();
	void InitPIDControllers();
	
	//Advanced Motor Control
	void Drive(float,float,float,float);
	void Stop();
	void ManipUp();
	void ManipDown();
	void ManipStop();
	void ArmUp();
	void ArmDown();
	void StopArm();
	void MoveArm(double);
	//void SpinningWheelStop();
	//void SpinningWheelOut();
	//void SpinningWheelIn();
	//void SpinningWheelRotateUp();
	//void SpinningWheelRotateDown();
	//void ArmAngle(); 
};
